#include <mc_path_planner/graph_planner/a_star.h>


struct CompareNodes {
  bool operator()(const Node2D* lhs, const Node2D* rhs) const {
    return lhs->getF() > rhs->getF() || (lhs->getF() == rhs->getF() && lhs->getH() > rhs->getH());
  }
};


AstarPlanner::AstarPlanner(boost::shared_ptr<MapBase> map_handler_ptr):
    PlannerBase(map_handler_ptr){
    GLOG_INFO <<  "[AstarPlanner] init......"; 
}

AstarPlanner::~AstarPlanner(){

}

// 路径规划
mc_status AstarPlanner::pathGenerate(Point2i start, Point2i goal, vector<Point2i>& points){
    mc_status status;
    points.clear();
    int width = mapHandlerPtr_->getWidth();
    int height = mapHandlerPtr_->getHeight();

    Node2D nodeStart(start.x, start.y, 0, 0, nullptr);
    Node2D nodeGoal(goal.x, goal.y, 0, 0, nullptr);
    nodeStart.updateH(nodeGoal);

    std::priority_queue<Node2D*, std::vector<Node2D*>, CompareNodes> openList;
    std::unordered_map<int, Node2D> closedList;
    openList.push(&nodeStart);
    GLOG_INFO << "openList size: " << openList.size();
    
    while(!ros::isShuttingDown() && !openList.empty()){
        // 获取优先级最高节点
        Node2D* nodePred = openList.top();
        openList.pop();
        int idxPred = nodePred->setIdx(width);

        // GLOG_INFO << "idxPred: " << idxPred 
        //     << ", x: " << nodePred->getX() 
        //     << ", y: " << nodePred->getY();

        if(closedList.find(idxPred) != closedList.end()){
            continue;
        }
        closedList[idxPred] = *nodePred;

        // 发现目标节点
        if(*nodePred == nodeGoal){
            GLOG_INFO << "find goal.";
            while(nodePred->getPred() != nullptr){
                points.emplace_back(Point2i(nodePred->getX(), nodePred->getY()));
                nodePred = nodePred->getPred();
            }  
            std::reverse(points.begin(), points.end());   
            GLOG_INFO << "points size: " << points.size();
            break;
        }

        // 遍历邻域节点
        for(int i = 0; i < Node2D::dir; i++) {
            Node2D* nodeSucc = nodePred->createSuccessor(i);
            int idxSucc = nodeSucc->setIdx(width);

            // 障碍物检测 是否在闭列表
            if(!mapHandlerPtr_->IsValid(Point2i(nodeSucc->getX(), nodeSucc->getY())) 
                || !mapHandlerPtr_->ValueIsValid(Point2i(nodeSucc->getX(), nodeSucc->getY()))
                || closedList.find(idxSucc) != closedList.end()){
                delete nodeSucc;
                continue;
            }

            // G值比较
            nodeSucc->updateG();
            float newG = nodeSucc->getG();
            if(closedList.find(idxSucc) == closedList.end() || newG < closedList[idxSucc].getG()) {
                nodeSucc->updateH(nodeGoal);
                // GLOG_INFO << "--> i: " << i 
                //     << ", idxSucc: " << idxSucc 
                //     << ", x: " << nodeSucc->getX() 
                //     << ", y: " << nodeSucc->getY();
                openList.push(nodeSucc);
            }else
            delete nodeSucc;
        }
    }

    return status;
}